%startup_rvc
clear;clc;close all

addpath(genpath('demo'),genpath('my_trajectory_plan'))


demo_bring_cup_with_matlab_jtraj=0;  %调用matlab的方法进行轨迹规划
demo_my_trajectory_plan=1;   %自己写的轨迹规划代码

if demo_bring_cup_with_matlab_jtraj+demo_my_trajectory_plan>1
    error('run one function one time 一次运行一个选项')
end

% demo_csdn=0;


%%
%位置信息，
% 定义齐次变换矩阵
T6_c = [0 0 1 0; 0 -1 0 0; 1 0 0 -0.05; 0 0 0 1];
% 计算各个点的齐次变换矩阵

%%%参数说明，前三个参数为alpha, beta, gamma: 欧拉角，表示末端执行器的姿态， 4 5 6表示x，y，z位置
points.p1 = util_get_end_joint_matric(0, 0, 25, -0.6, -0.4, -0.5, T6_c);    %点1(初始点)
points.p2 = util_get_end_joint_matric(0, 0, 35, -0.6, -0.4, 0, T6_c);       %点2
points.p3 = util_get_end_joint_matric(0, -60, 0, 0.6, -0.4, 0, T6_c);       %点3
points.pf = util_get_end_joint_matric(0, -50, 0, 0.3, -0.4, -0.2, T6_c);    %点4(目标点位)

%%

if demo_bring_cup_with_matlab_jtraj
    %使用matlab的轨迹规划
    bring_cup_with_matlab_jtraj(points);
end

if demo_my_trajectory_plan
    bring_cup_with_my_trajectory(points);
end


%%